#include "bmi160.h"
#include "device.h"

#define VHW_IIC_BMI160 vIIC_4

#define IIC_ADDR_BMI160 0x68

static void delay_ms(uint32_t ms)
{
	Device_DelayMs(ms);
}

static unsigned char bmi160_read(unsigned char reg, unsigned char *data, unsigned char len)
{
	if (Device_Write(VHW_IIC_BMI160, &reg, 1, IIC_ADDR_BMI160) == 0)
	{
		return (Device_Read(VHW_IIC_BMI160, data, len, IIC_ADDR_BMI160) == 0) ? 1 : 0;
	}
	return 0;
}

static unsigned char bmi160_write(unsigned char reg, unsigned char *data, unsigned char len)
{
	unsigned char buffer[20] = {reg};

	memcpy(&buffer[1], data, len);
	return (Device_Write(VHW_IIC_BMI160, buffer, len + 1, IIC_ADDR_BMI160) == 0) ? 1 : 0;
}

static unsigned char bmi160_set_command_register(unsigned char val)
{
	return bmi160_write(BMI160_CMD_COMMANDS_ADDR, &val, 1);
}

unsigned char bmi160_init(void)
{
	unsigned char value, res, n, t;

	n = 5;
	while (1)
	{
		if (--n == 0)
			return 0;

		bmi160_set_command_register(DRIVE_SOFTRESET);
		delay_ms(15);
		bmi160_set_command_register(DRIVE_SOFTRESET);
		delay_ms(15);

		if (bmi160_read(BMI160_USER_CHIP_ID_ADDR, &value, 1) == 0)
			continue;
		if (0xD1 != value)
			continue;

		delay_ms(1);
		value = (BMI160_GYRO_NORMAL_MODE << 4) | BMI160_GYRO_OUTPUT_DATA_RATE_100HZ;
		if (bmi160_write(BMI160_USER_GYRO_CONFIG_ADDR, &value, 1) == 0)
			continue;
		delay_ms(1);
		if (bmi160_read(BMI160_USER_GYRO_CONFIG_ADDR, &res, 1) == 0)
			continue;
		if (value != res)
			continue;
		delay_ms(1);
		value = BMI160_GYRO_RANGE_500_DEG_SEC;
		if (bmi160_write(BMI160_USER_GYRO_RANGE_ADDR, &value, 1) == 0)
			continue;
		delay_ms(1);
		if (bmi160_read(BMI160_USER_GYRO_RANGE_ADDR, &res, 1) == 0)
			continue;
		if (value != res)
			continue;
		delay_ms(1);
		value = 0x28;
		if (bmi160_write(BMI160_USER_ACCEL_CONFIG_ADDR, &value, 1) == 0)
			continue;
		delay_ms(1);
		if (bmi160_read(BMI160_USER_ACCEL_CONFIG_ADDR, &res, 1) == 0)
			continue;
		if (value != res)
			continue;
		delay_ms(1);
		value = 0x03;
		if (bmi160_write(BMI160_USER_ACCEL_RANGE_ADDR, &value, 1) == 0)
			continue;
		delay_ms(1);
		if (bmi160_read(BMI160_USER_ACCEL_RANGE_ADDR, &res, 1) == 0)
			continue;
		if (value != res)
			continue;
		delay_ms(1);
		if (bmi160_set_command_register(ACCEL_MODE_NORMAL) == 0)
			continue;
		for (t = 1; t < 80; t++)
		{
			delay_ms(1);
			if (bmi160_read(BMI160_USER_PMU_STAT_ADDR, &res, 1) == 0)
				break;
			if (0x10 == (res & 0x30))
			{
				t = 0;
				break;
			}
		}
		if (t != 0)
			continue;
		delay_ms(1);
		if (bmi160_set_command_register(GYRO_MODE_NORMAL) == 0)
			continue;
		for (t = 1; t < 80; t++)
		{
			delay_ms(1);
			if (bmi160_read(BMI160_USER_PMU_STAT_ADDR, &res, 1) == 0)
				break;
			if (0x04 == (res & 0x06))
			{
				t = 0;
				break;
			}
		}
		if (t != 0)
			continue;

		break;
	}
	return 1;
}

unsigned char bmi160_read_gyroAcc_rate(short acc[3], short gyr[3])
{
	unsigned char rbuf[12] = {0};
	if (bmi160_read(BMI160_USER_DATA_8_ADDR, rbuf, 12) == 0)
		return 0;
	gyr[0] = rbuf[0] | (rbuf[1] << 8);
	gyr[1] = rbuf[2] | (rbuf[3] << 8);
	gyr[2] = rbuf[4] | (rbuf[5] << 8);
	acc[0] = rbuf[6] | (rbuf[7] << 8);
	acc[1] = rbuf[8] | (rbuf[9] << 8);
	acc[2] = rbuf[10] | (rbuf[11] << 8);
	return 1;
}

unsigned char bmi160_lowpowermode(void)
{
	if (bmi160_set_command_register(GYRO_MODE_SUSPEND) == 0)
		return 0;
	if (bmi160_set_command_register(ACCEL_MODE_SUSPEND) == 0)
		return 0;
	return 1;
}
